#ifndef __UDSF_SYS_API_C__
#define __UDSF_SYS_API_C__

#include "app.h"

/*
*   notice:
*   UDSF 同UDF编程：不可调用cstd-lib,只可调用"sdk_ifs_udk_cfg.h"、"sys_api.h"中列出的API函数。
*   UDSF文件内函数均需使用static修饰符，且UDF中以#include "udsf_xxx.c"形式引用

本函数复刻 radar.lib 中 部分函数（应用中要求执行时间短）
*/

static void udsf_prepdata_handle(void) __attribute__((unused));
static void udsf_copywin_handle(uint16_t *srcAddr,WIN_CFG *sCfg) __attribute__((unused));
static uint32_t udsf_cfar_handle(CFAR_CONFIG *sCfg) __attribute__((unused));
static void udsf_angle_seq_pre(PARAM_DATAPREP *sCfg) __attribute__((unused));
static void udsf_fft3d_pre_handle(FFT3D_PRE *sCfg,struct pc_rst *sRst) __attribute__((unused));
static void udsf_bbe_waitisr(uint32_t val) __attribute__((unused));
static void udsf_rf_venus_deinit(void) __attribute__((unused));
static void udsf_p2_fft_handle(P2_FFT_CONFIG *sCfg) __attribute__((unused));
static void udsf_p2_fft_stepcfg8_handle(P2_FFT_CONFIG *sCfg,uint32_t Step) __attribute__((unused));
static void udsf_bbe_prepinit(PARAM_DATAPREP *sCfg) __attribute__((unused));
static void udsf_rf_venus_init(void) __attribute__((unused));
static void udsf_anacfg_set(ANACFG *sCfg) __attribute__((unused));
static void udsf_wdt_rstnow(void) __attribute__((unused));
static void udsf_cmphit_cfar_target(uint16_t aLen,BBE_CFARRST_T *aArray,uint16_t bLen,BBE_CFARRST_T *bArray,_Bool *hitAFlag,_Bool *hitBFlag,uint16_t cfar_max_num) __attribute__((unused));

extern const uint16_t SDATA_winRange[MAX_WIN_RANGE];
extern const uint16_t SDATA_winVel[MAX_WIN_VEL];
extern const int SDATA_afftArrayPre[NUM_ANGLE*2];

void udsf_prepdata_handle(void){
	/*angleseq pre*/
	if(paramDataPrep->constEn&CONST_ANGLE_EN){
		udsf_angle_seq_pre(paramDataPrep);
	} 
	if(paramDataPrep->constEn&CONST_WINRANGE_EN){
		udsf_copywin_handle((uint16_t *)&SDATA_winRange[0],&paramDataPrep->winRange);
	}
	if(paramDataPrep->constEn&CONST_WINVEL_EN){
		udsf_copywin_handle((uint16_t *)&SDATA_winVel[0],&paramDataPrep->winVel);
	}
}

void udsf_copywin_handle(uint16_t *srcAddr,WIN_CFG *sCfg){
	uint8_t *dstBuf=(uint8_t *)(0x30000000+sCfg->dstOffsetAddr);
	uint8_t *srcBuf=(uint8_t *)srcAddr;
	for(uint32_t i=0;i<sCfg->len;i++){
			*(uint16_t *)dstBuf=*(uint16_t *)srcBuf;
			dstBuf+=sCfg->dstInc;
			srcBuf+=sCfg->srcInc;		
	}
}

uint32_t udsf_cfar_handle(CFAR_CONFIG *sCfg){
	uint32_t rtn;
	bbe_regb_str->CFAR_CFG0=sCfg->cfarCfg0;
	bbe_regb_str->CFAR_CFG1=sCfg->cfarCfg1;
	bbe_regb_str->CFAR_CFG2=sCfg->cfarCfg2;
	bbe_regb_str->CFAR_CFG3=sCfg->cfarCfg3;
	bbe_regb_str->CFAR_CFG4=sCfg->cfarCfg4;
	bbe_regb_str->CFAR_CFG5=sCfg->cfarCfg5;
	bbe_regb_str->CFAR_CFG6=sCfg->cfarCfg6;
	bbe_regb_str->CFAR_CFG7=sCfg->cfarCfg7;
	bbe_regb_str->CFAR_CFG8=sCfg->cfarCfg8;	
	BBE_ISR_CLEAR(BBE_ISR_CFAR_END);
	BBE_OPGATE_EN(BBE_GATE_CFAR);
	BBE_OPTRIG(BBE_TRIG_CFAR);
	udsf_bbe_waitisr(BBE_ISR_CFAR_END);
	BBE_OPGATE_DIS(BBE_GATE_CFAR);
	rtn = bbe_regb_str->CFAR_STA0;
	return rtn;
}

void udsf_angle_seq_pre(PARAM_DATAPREP *sCfg){
	int32_t *src0Addr=(int32_t *)(BBE_ABUF0_CMPLX_BASE); //seq
	int32_t *src1Addr=(int32_t *)(BBE_ABUF1_CMPLX_BASE); //ap
	for(uint32_t i=0;i<NUM_ANGLE;i++){
		src0Addr[i*4]=8192; //8192=1
		src0Addr[i*4+1]=0;
		src0Addr[i*4+2]=SDATA_afftArrayPre[i*2];
		src0Addr[i*4+3]=SDATA_afftArrayPre[i*2+1];
	}
	src1Addr[0]=8192; //8192=1
	src1Addr[1]=0;
	src1Addr[2]=sCfg->calibAnt2_r;
	src1Addr[3]=sCfg->calibAnt2_i;	//-8096,-3755
	BBE_P2_CFG0(ANT_MAX_NUM-1,P2_FIX2PF_LSFB_0BIT,P2_MAX_CLR_EN,P2_FFT_MODE_DIS,P2_DMA_DSIZE_32BIT,P2_RSF_DIV1,P2_SUBMODE_0,P2_MODE_MUL); //intraCnt,p2_fft_mode,dma_dsize,rsfBit,sub_mode,mode
	BBE_P2_CFG1(0,NUM_ANGLE-1);//jumpCnt,interCnt
	BBE_P2_CFG2(0,0);//ncoMode,nco_fcw
	BBE_P2_CFG3(0,NUM_BYTE32*ANT_MAX_NUM);//src0JumpInc,src0InterInc
	BBE_P2_CFG4(NUM_BYTE32,BBE_ABUF0_OFFSET);//src0IntraInc,src0BaseAddr
	BBE_P2_CFG5(0,0);//src1JumpInc,src1InterInc
	BBE_P2_CFG6(NUM_BYTE32,BBE_ABUF1_OFFSET);//src1IntraInc,src1BaseAddr
	BBE_P2_CFG7(0,NUM_BYTE32*ANT_MAX_NUM);//dstJumpInc,dstInterInc
	BBE_P2_CFG8(NUM_BYTE32,sCfg->angleDstAddr);//dstIntraInc,dstBaseAddr		
	
	BBE_ISR_CLEAR(BBE_ISR_P2_JUMP_END);
	BBE_OPGATE_EN(BBE_GATE_P2);
	BBE_OPTRIG(BBE_TRIG_P2);	
	udsf_bbe_waitisr(BBE_ISR_P2_JUMP_END);
}

#if defined(FMCW_RADAR_4D)
static void udsf_fft3d_pre_handle(FFT3D_PRE *sCfg,struct pc_rst_4d *sRst)
#else
static void udsf_fft3d_pre_handle(FFT3D_PRE *sCfg,struct pc_rst *sRst)
#endif
{
	uint32_t *inBuf=(uint32_t *)(sCfg->srcAddr);
	uint32_t *outBuf=(uint32_t *)(sCfg->dstAddr);
	for(uint32_t cnt=0;cnt<sRst->targetNum;cnt++){
		outBuf[cnt*2]=inBuf[sRst->target[cnt].d2Idx*sCfg->interCnt+sRst->target[cnt].d1Idx*sCfg->intraCnt];
	}
}


void udsf_bbe_waitisr(uint32_t val){
	while( (bbe_regb_str->ISR & val)==0 );
	bbe_regb_str->ISR = val;	
}

void udsf_rf_venus_deinit(void){
	ana_Venus_st->ramp_cmd=VENUS_WAVE_STOP;
}

void udsf_p2_fft_handle(P2_FFT_CONFIG *sCfg){
	bbe_regb_str->P2_CFG0=sCfg->p2Cfg0;
	bbe_regb_str->P2_CFG1=sCfg->p2Cfg1;
	bbe_regb_str->P2_CFG2=sCfg->p2Cfg2;
	bbe_regb_str->P2_CFG3=sCfg->p2Cfg3;
	bbe_regb_str->P2_CFG4=sCfg->p2Cfg4;
	bbe_regb_str->P2_CFG5=sCfg->p2Cfg5;
	bbe_regb_str->P2_CFG6=sCfg->p2Cfg6;
	bbe_regb_str->P2_CFG7=sCfg->p2Cfg7;
	bbe_regb_str->P2_CFG8=sCfg->p2Cfg8;
	BBE_ISR_CLEAR(BBE_ISR_P2_JUMP_END|BBE_ISR_FFT_UNLOAD_END);
	if((bbe_regb_str->P2_CFG0>>BB_BIT14_POS)&P2_FFT_MODE_EN){
		bbe_regb_str->FFT_CFG0=sCfg->fftCfg0;
		bbe_regb_str->FFT_CFG1=sCfg->fftCfg1;
		bbe_regb_str->FFT_CFG2=sCfg->fftCfg2;		
		BBE_OPGATE_EN(BBE_GATE_P2|BBE_GATE_FFT);
	}else{
		BBE_OPGATE_EN(BBE_GATE_P2);
	}
	BBE_OPTRIG(BBE_TRIG_P2);
	udsf_bbe_waitisr(BBE_ISR_P2_JUMP_END);
	BBE_OPGATE_DIS(BBE_GATE_P2|BBE_GATE_FFT);
}

void udsf_p2_fft_stepcfg8_handle(P2_FFT_CONFIG *sCfg,uint32_t Step){
	bbe_regb_str->P2_CFG0=sCfg->p2Cfg0;
	bbe_regb_str->P2_CFG1=sCfg->p2Cfg1;
	bbe_regb_str->P2_CFG2=sCfg->p2Cfg2;
	bbe_regb_str->P2_CFG3=sCfg->p2Cfg3;
	bbe_regb_str->P2_CFG4=sCfg->p2Cfg4;
	bbe_regb_str->P2_CFG5=sCfg->p2Cfg5;
	bbe_regb_str->P2_CFG6=sCfg->p2Cfg6;
	bbe_regb_str->P2_CFG7=sCfg->p2Cfg7;
	bbe_regb_str->P2_CFG8=sCfg->p2Cfg8+Step;
	BBE_ISR_CLEAR(BBE_ISR_P2_JUMP_END|BBE_ISR_FFT_UNLOAD_END);
	if((bbe_regb_str->P2_CFG0>>BB_BIT14_POS)&P2_FFT_MODE_EN){
		bbe_regb_str->FFT_CFG0=sCfg->fftCfg0;
		bbe_regb_str->FFT_CFG1=sCfg->fftCfg1;
		bbe_regb_str->FFT_CFG2=sCfg->fftCfg2;		
		BBE_OPGATE_EN(BBE_GATE_P2|BBE_GATE_FFT);
	}else{
		BBE_OPGATE_EN(BBE_GATE_P2);
	}
	BBE_OPTRIG(BBE_TRIG_P2);
	udsf_bbe_waitisr(BBE_ISR_P2_JUMP_END);
	BBE_OPGATE_DIS(BBE_GATE_P2|BBE_GATE_FFT);	
}

void udsf_bbe_prepinit(PARAM_DATAPREP *sCfg){
	BBE_OPGATE_DIS(BBE_GATE_PREP);
	BBE_OPCLEAR(BBE_CLEAR_PREP);		
	BBE_ISR_CLEAR(BBE_ISR_RAMP_PEDGE|BBE_ISR_PREP_FRAME_END|BBE_ISR_PREP_RAMP_END);
	bbe_regb_str->PREP_CFG0=sCfg->prepCfg0;
	bbe_regb_str->PREP_CFG1=sCfg->prepCfg1;
	bbe_regb_str->PREP_CFG2=sCfg->prepCfg2;	
	BBE_OPGATE_EN(BBE_GATE_PREP);
}
#if !defined(USE_EX_RF)
void udsf_rf_venus_init(void){
	ana_Venus_st->ramp_pll_cfg=paramANA_Venus->ramp_pll_cfg;
	ana_Venus_st->ramp_syncw_cfg=paramANA_Venus->ramp_syncw_cfg;
	ana_Venus_st->ramp_chirpw_cfg=paramANA_Venus->ramp_chirpw_cfg;
	ana_Venus_st->ramp_wm_cfg0=paramANA_Venus->ramp_wm_cfg0;
	ana_Venus_st->ramp_mem03=paramANA_Venus->ramp_mem03;
	ana_Venus_st->ramp_mem02=paramANA_Venus->ramp_mem02;
	ana_Venus_st->ramp_mem01=paramANA_Venus->ramp_mem01;
	ana_Venus_st->ramp_mem00=paramANA_Venus->ramp_mem00;
	ana_Venus_st->ramp_mem13=paramANA_Venus->ramp_mem13;
	ana_Venus_st->ramp_mem12=paramANA_Venus->ramp_mem12;
	ana_Venus_st->ramp_mem11=paramANA_Venus->ramp_mem11;
	ana_Venus_st->ramp_mem10=paramANA_Venus->ramp_mem10;
}
#endif

void udsf_anacfg_set(ANACFG *sCfg){
	ana_Venus_st->ana_cfg0=sCfg->ana_cfg0;
	ana_Venus_st->ana_cfg1=sCfg->ana_cfg1;
	ana_Venus_st->ana_cfg2=sCfg->ana_cfg2;
}

void udsf_wdt_rstnow(void){
	SYSC->CLK_EN |= WDT_PATTERN_EN_Msk;
	WDT->CMP = 0;	
	WDT->STR = 0x1;
}

void udsf_cmphit_cfar_target(uint16_t aLen,BBE_CFARRST_T *aArray,uint16_t bLen,BBE_CFARRST_T *bArray,_Bool *hitAFlag,_Bool *hitBFlag,uint16_t cfar_max_num){

	for( int i=0;i<cfar_max_num;i++ ){
		hitAFlag[i] = 0;
		hitBFlag[i] = 0;
	}
	
	for(uint32_t i=0;i<aLen;i++){
		for(uint32_t j=0;j<bLen;j++){
			if(!hitBFlag[j]){
				if((aArray[i].interIdx == bArray[j].intraIdx)&&(aArray[i].intraIdx == bArray[j].interIdx)){
					hitAFlag[i]=1;
					hitBFlag[j]=1;
				}
			}
		}
	}
}

#if !defined(FMCW_RADAR_4D)
#define DBSCAN_NOISE          ( -1 )
#define DBSCAN_IDLE           ( 0 )
static void udsf_dbscan_cluster_proc(struct dbscan_cluster *pDBSCAN,struct pc_rst *frameRst,uint8_t vel_num)
{
//	for(int i=0;i<sizeof(struct dbscan);i++){
//		((uint8_t*)pDBSCAN)[i] = 0;
//	}
	DBSCANFRAME  *pDBSCAN_frame  = pDBSCAN->dbscanFrame;
	DBSCANOUT    *pDBSCAN_clu    = pDBSCAN->dbscanClu;
	DBSCANNB     *pDBSCAN_nbA    = pDBSCAN->nbA;
	DBSCANNB     *pDBSCAN_nbB    = pDBSCAN->nbB;
	DBSCANCFG    *pDBSCAN_cfg    = pDBSCAN->cfg;

	frameRst->targetNum > pDBSCAN->frame_max_num ? (pDBSCAN_frame->ptNum = pDBSCAN->frame_max_num) : (pDBSCAN_frame->ptNum = frameRst->targetNum);
	// for ( int i=0;i<frameRst->targetNum;i++ ){
	// 	CSTD_PRINTF("%d,%d,%d,%d\n",i,frameRst->target[i].d1Idx,frameRst->target[i].d2Idx,frameRst->target[i].d3Idx);
	// }
	for (int i = 0; i < pDBSCAN_frame->ptNum; i++)
	{
		pDBSCAN_frame->pt[i].rIdx = frameRst->target[i].d1Idx;
		if (frameRst->target[i].d2Idx*2 >= vel_num)
			pDBSCAN_frame->pt[i].vIdx = frameRst->target[i].d2Idx - vel_num;
		else
			pDBSCAN_frame->pt[i].vIdx = frameRst->target[i].d2Idx;
		if (frameRst->target[i].d3Idx*2 >= NUM_ANGLE )
			pDBSCAN_frame->pt[i].sinPhiIdx = frameRst->target[i].d3Idx - NUM_ANGLE;
		else
			pDBSCAN_frame->pt[i].sinPhiIdx = frameRst->target[i].d3Idx;
		pDBSCAN_frame->pt[i].abs = frameRst->target[i].powABS;
	}

	int len = pDBSCAN_frame->ptNum;

	// dbscan init
	int clusterID = 0;
	int isNB = 0;

	for (int i = 0; i < len; i++)
	{
		pDBSCAN_frame->pt[i].visited   = 0;
		pDBSCAN_frame->pt[i].corePt    = 0;
		pDBSCAN_frame->pt[i].clusterID = DBSCAN_IDLE;
	}
	pDBSCAN_nbA->ptNum = 0;
	pDBSCAN_nbB->ptNum = 0;
					
	// dbscan
	for (int i = 0; i < len; i++)
	{
		if (pDBSCAN_frame->pt[i].visited == 0)
		{
			pDBSCAN_frame->pt[i].visited = 1;

			// DBSCAN_regionQuery Start
			int neighborPts = 0;
			pDBSCAN_nbA->ptNum = 0;
			for (int j = 0; j < len; j++)
			{
				//DBSCAN_isNB Start
				isNB = 0;
				int absVal = pDBSCAN_frame->pt[i].rIdx - pDBSCAN_frame->pt[j].rIdx > 0 ? (pDBSCAN_frame->pt[i].rIdx - pDBSCAN_frame->pt[j].rIdx) : (pDBSCAN_frame->pt[j].rIdx - pDBSCAN_frame->pt[i].rIdx);
				if (absVal < pDBSCAN_cfg->epsRIdx)
				{
					absVal = pDBSCAN_frame->pt[i].vIdx - pDBSCAN_frame->pt[j].vIdx > 0 ? (pDBSCAN_frame->pt[i].vIdx - pDBSCAN_frame->pt[j].vIdx) : (pDBSCAN_frame->pt[j].vIdx - pDBSCAN_frame->pt[i].vIdx);
					if (absVal < pDBSCAN_cfg->epsVIdx)
					{
						absVal = pDBSCAN_frame->pt[i].sinPhiIdx - pDBSCAN_frame->pt[j].sinPhiIdx > 0 ? (pDBSCAN_frame->pt[i].sinPhiIdx - pDBSCAN_frame->pt[j].sinPhiIdx) : (pDBSCAN_frame->pt[j].sinPhiIdx - pDBSCAN_frame->pt[i].sinPhiIdx);
						if (absVal < pDBSCAN_cfg->epsSinPhiIdx)
						{
							isNB = 1;
						}
					}
				}
				//DBSCAN_isNB End
				if (isNB)
				{
					neighborPts++;
					pDBSCAN_nbA->idx[pDBSCAN_nbA->ptNum] = j;
					pDBSCAN_nbA->ptNum++;
				}
			}
			// DBSCAN_regionQuery End

			if (neighborPts < pDBSCAN_cfg->minPts)
			{
				pDBSCAN_frame->pt[i].clusterID = DBSCAN_NOISE;
			}
			else
			{
				clusterID++;
				pDBSCAN_frame->pt[i].corePt    = 1;
				pDBSCAN_frame->pt[i].clusterID = (int8_t)clusterID;
				for (int j = 0; j < pDBSCAN_nbA->ptNum; j++)
				{
					if (pDBSCAN_frame->pt[pDBSCAN_nbA->idx[j]].visited == 0)
					{
						pDBSCAN_frame->pt[pDBSCAN_nbA->idx[j]].visited = 1;

						// DBSCAN_regionQuery Start
						neighborPts = 0;
						pDBSCAN_nbB->ptNum = 0;
						for (int k = 0; k < len; k++)
						{
							//DBSCAN_isNB Start
							isNB = 0;
							int absVal = pDBSCAN_frame->pt[pDBSCAN_nbA->idx[j]].rIdx - pDBSCAN_frame->pt[k].rIdx > 0 ? (pDBSCAN_frame->pt[pDBSCAN_nbA->idx[j]].rIdx - pDBSCAN_frame->pt[k].rIdx) : (pDBSCAN_frame->pt[k].rIdx - pDBSCAN_frame->pt[pDBSCAN_nbA->idx[j]].rIdx);
							if (absVal < pDBSCAN_cfg->epsRIdx)
							{
								absVal = pDBSCAN_frame->pt[pDBSCAN_nbA->idx[j]].vIdx - pDBSCAN_frame->pt[k].vIdx > 0 ? (pDBSCAN_frame->pt[pDBSCAN_nbA->idx[j]].vIdx - pDBSCAN_frame->pt[k].vIdx) : (pDBSCAN_frame->pt[k].vIdx - pDBSCAN_frame->pt[pDBSCAN_nbA->idx[j]].vIdx);
								if (absVal < pDBSCAN_cfg->epsVIdx)
								{
									absVal = pDBSCAN_frame->pt[pDBSCAN_nbA->idx[j]].sinPhiIdx - pDBSCAN_frame->pt[k].sinPhiIdx > 0 ? (pDBSCAN_frame->pt[pDBSCAN_nbA->idx[j]].sinPhiIdx - pDBSCAN_frame->pt[k].sinPhiIdx) : (pDBSCAN_frame->pt[k].sinPhiIdx - pDBSCAN_frame->pt[pDBSCAN_nbA->idx[j]].sinPhiIdx);
									if (absVal < pDBSCAN_cfg->epsSinPhiIdx)
									{
										isNB = 1;
									}
								}
							}
							//DBSCAN_isNB End
							if (isNB)
							{
								neighborPts++;
								pDBSCAN_nbB->idx[pDBSCAN_nbB->ptNum] = k;
								pDBSCAN_nbB->ptNum++;
							}
						}
						// DBSCAN_regionQuery End

						if (neighborPts >= pDBSCAN_cfg->minPts)
						{
							// DBSCAN_neighborUnion Start
							for (int m = 0; m < pDBSCAN_nbB->ptNum; m++)
							{
								int n;
								for ( n = 0; n < pDBSCAN_nbA->ptNum; n++)
								{
									if (pDBSCAN_nbB->idx[m] == pDBSCAN_nbA->idx[n])
										break;
								}
								if (n == pDBSCAN_nbA->ptNum)
								{
									pDBSCAN_nbA->idx[pDBSCAN_nbA->ptNum] = pDBSCAN_nbB->idx[m];
									pDBSCAN_nbA->ptNum++;
								}
							}
							// DBSCAN_neighborUnion End
						}
					}
					if (pDBSCAN_frame->pt[pDBSCAN_nbA->idx[j]].clusterID == DBSCAN_IDLE)
					{
						pDBSCAN_frame->pt[pDBSCAN_nbA->idx[j]].clusterID = (int8_t)clusterID;
					}
				}
			}
		}
	}

	/* dbscan_ClusterRpt */
	// find clusterID max
	int index = 0;
	for (int i = 1; i < len; i++)
	{
		if (pDBSCAN_frame->pt[index].clusterID < pDBSCAN_frame->pt[i].clusterID)
			index = i;
	}
	pDBSCAN_clu->numCluster = pDBSCAN_frame->pt[index].clusterID;

	if (pDBSCAN_clu->numCluster == 0xFF){
		pDBSCAN_clu->numCluster = 0;
	}
	
	for (int i = 1; i <= pDBSCAN_clu->numCluster; i++)
	{
		int numPt           = 0;
		int rIdxMean        = 0;
		int vIdxMean        = 0;
		int sinPhiIdxMean   = 0;
		uint32_t abs_total  = 0;
		uint32_t abs_tmp    = 0;	
		
		for (int j = 0; j < len; j++)
		{
			if (pDBSCAN_frame->pt[j].clusterID == i)
			{
				if( numPt <= pDBSCAN_cfg->maxPts ){
					rIdxMean        = rIdxMean      + pDBSCAN_frame->pt[j].rIdx;
					vIdxMean        = vIdxMean      + pDBSCAN_frame->pt[j].vIdx;
					sinPhiIdxMean   = sinPhiIdxMean + pDBSCAN_frame->pt[j].sinPhiIdx;
					abs_tmp = abs_total + pDBSCAN_frame->pt[j].abs;
					if( abs_tmp > abs_total ){
						abs_total = abs_tmp;
					}
					numPt           = numPt + 1;
				}
			}
		}

		//CSTD_PRINTF("dbscan sum %d,%d,%d,%d\n",rIdxMean,vIdxMean,sinPhiIdxMean,numPt);
		//CSTD_PRINTF("maxpts %d\n",pDBSCAN_cfg->maxPts);
		pDBSCAN_clu->cluster[i - 1].rIdxMean      = rIdxMean * 4096 / numPt;
		pDBSCAN_clu->cluster[i - 1].vIdxMean      = vIdxMean * 4096 / numPt;
		pDBSCAN_clu->cluster[i - 1].sinPhiIdxMean = sinPhiIdxMean * 4096 / numPt;
		pDBSCAN_clu->cluster[i - 1].abs           = abs_total;
		//CSTD_PRINTF("dbscan:%d,%d,%d,%d\n",i-1,pDBSCAN_clu->cluster[i - 1].rIdxMean,pDBSCAN_clu->cluster[i - 1].vIdxMean,pDBSCAN_clu->cluster[i - 1].sinPhiIdxMean);
	}
}
static int udsf_alpha_filter_proc(struct alpha_filter_cfg *p_alpha_filter_cfg,struct alpha_filter *p_alpha_filter,struct epc *p_epc,uint32_t sn,uint8_t info1,uint8_t info2)
{
	uint32_t FILTER_RTH              = p_alpha_filter_cfg->FILTER_RTH;
	uint32_t FILTER_VTH              = p_alpha_filter_cfg->FILTER_VTH;
	uint32_t FILTER_ATH              = p_alpha_filter_cfg->FILTER_ATH;
	uint8_t  FILTER_VALID_CNT        = p_alpha_filter_cfg->FILTER_VALID_CNT;
	
	int8_t  ALPHA_R                  = p_alpha_filter_cfg->ALPHA_R;
	int8_t  ALPHA_V                  = p_alpha_filter_cfg->ALPHA_V;
	int8_t  ALPHA_A                  = p_alpha_filter_cfg->ALPHA_A;
	uint8_t  FILTER_TIMEOUT_CNT      = p_alpha_filter_cfg->FILTER_TIMEOUT_CNT;
	
	int8_t  EX_FACTOR                = p_alpha_filter_cfg->EX_FACTOR_RV;
	int8_t  EX_FACTOR_A              = p_alpha_filter_cfg->EX_FACTOR_A;
	uint8_t  UPDATE_MODE             = p_alpha_filter_cfg->UPDATE_MODE;

	struct alpha_target *p_alpha_target = p_alpha_filter->p_target; 
	struct epc_target  * p_epc_target = p_epc->p_target;

	// 全部输入点标记为未处理
	for( int in=0;in<p_epc->target_cnt;in++){
			p_epc_target[in].unprocess_flag = 1;
	}
	
	struct alpha_target_ordered *ordered_table = p_alpha_filter->p_ordered;
	
	// 全部已有点标记为未处理及排序
	for( int alpha_cnt=0;alpha_cnt<p_alpha_filter->cnt;alpha_cnt++ ){
		p_alpha_target[alpha_cnt].unprocess_flag = 1;
		
		ordered_table[alpha_cnt].tick = p_alpha_target[alpha_cnt].tick_start;
		ordered_table[alpha_cnt].index = alpha_cnt;
	}

	// 轨迹创建时间排序
	for (int i = 0; i < p_alpha_filter->cnt-1; i++) {
		for (int j = 0; j < p_alpha_filter->cnt-i-1; j++) {
			if (ordered_table[j].tick > ordered_table[j+1].tick) {
					struct alpha_target_ordered temp = ordered_table[j];
					ordered_table[j] = ordered_table[j+1];
					ordered_table[j+1] = temp;
			}
		}
	}
	
	// α滤波
	for( int ordered_i=0;ordered_i<p_alpha_filter->cnt;ordered_i++ ){

		int alpha_cnt = ordered_table[ordered_i].index;
		
		int update_epc_index = -1;
		
		uint32_t rIdx_min = UINT_MAX;
		int rIdx_min_index = -1;
		
		uint32_t vIdx_abs_max = 0;
		int vIdx_max_index = -1;
				
		for( int in=0;in<p_epc->target_cnt;in++){
			if( (p_epc_target[in].presence_flag) || (!p_epc_target[in].unprocess_flag) ){ // 只对未使用动点进行α滤波
					continue;
			}
			if(  (CSTD_ABS((int)(p_epc_target[in].rIdx*EX_FACTOR)        - (int)(p_alpha_target[alpha_cnt].rIdx))     < FILTER_RTH   ) &&
					 (CSTD_ABS((int)(p_epc_target[in].vIdx*EX_FACTOR)        - (int)(p_alpha_target[alpha_cnt].vIdx))     < FILTER_VTH   ) &&
					 (CSTD_ABS((int)(p_epc_target[in].sinPhiIdx*EX_FACTOR_A) - (int)(p_alpha_target[alpha_cnt].sinPhiIdx))< FILTER_ATH ) ){
					
					if( p_epc_target[in].rIdx < rIdx_min ){
						rIdx_min = p_epc_target[in].rIdx;
						rIdx_min_index = in;
					}
					
					uint32_t vIdx_abs = CSTD_ABS(p_epc_target[in].vIdx);
					if( vIdx_abs_max < vIdx_abs ){
						vIdx_abs_max = vIdx_abs;
						vIdx_max_index = in;
					}
						 
					p_epc_target[in].unprocess_flag = 0;
					//break;
			}
		}
		
		if( UPDATE_MODE == 0 ){
			update_epc_index = rIdx_min_index;
		}else{
			update_epc_index = vIdx_max_index;
		}
		
		if( p_alpha_target[alpha_cnt].unprocess_flag )
		{
			p_alpha_target[alpha_cnt].unprocess_flag = 0;
			     
			uint32_t r_old     = p_alpha_target[alpha_cnt].rIdx;
			int32_t v_old      = p_alpha_target[alpha_cnt].vIdx;
			int32_t a_old      = p_alpha_target[alpha_cnt].sinPhiIdx;

			#if 1
			int64_t s64_temp = v_old*p_epc->vel_res_mm_s*p_epc->frame_interval/1000;
			int r_temp = s64_temp / p_epc->range_res_mm;
			
//			float flt_temp = v_old*1.0f*p_epc->vel_res_mm_s*p_epc->frame_interval/1000;
//			int r_temp = flt_temp / p_epc->range_res_mm;
			/*32bit*/
			int32_t r_forecast = r_old + r_temp;
			#else
			int32_t r_forecast = r_old + v_old/100*(p_epc->vel_res_mm_s)*(p_epc->frame_interval/10)/((p_epc->range_res_mm));
			#endif
			int32_t v_forecast = v_old;
			int32_t a_forecast = a_old;
			
			if( r_forecast < 0 ){
				r_forecast = 0;
			}

			uint32_t	r_now     = r_forecast;
			int32_t		v_now     = v_forecast;
			int32_t		a_now     = a_forecast;
			
			if( update_epc_index >= 0){
				p_alpha_target[alpha_cnt].update_cnt ++;
				p_alpha_target[alpha_cnt].sn_rcd = sn+1; // 记录当前sn
				
				uint8_t info1 = p_alpha_target[alpha_cnt].info1;
				uint8_t info2 = p_alpha_target[alpha_cnt].info2;
				*(epc_unit_t*)&p_alpha_target[alpha_cnt] = *((epc_unit_t*)&p_epc_target[update_epc_index]);
				p_alpha_target[alpha_cnt].info1 = info1;
				p_alpha_target[alpha_cnt].info2 = info2;
				p_alpha_target[alpha_cnt].rIdx      *= EX_FACTOR;
				p_alpha_target[alpha_cnt].vIdx      *= EX_FACTOR;
				p_alpha_target[alpha_cnt].sinPhiIdx *= EX_FACTOR_A;
				
				int32_t r_observe  = p_alpha_target[alpha_cnt].rIdx;
				int32_t v_observe  = p_alpha_target[alpha_cnt].vIdx;
				int32_t a_observe  = p_alpha_target[alpha_cnt].sinPhiIdx;
			
				r_now       = (r_forecast*(100-ALPHA_R)+r_observe*ALPHA_R)/100;
				v_now       = (v_forecast*(100-ALPHA_V)+v_observe*ALPHA_V)/100;
				a_now       = (a_forecast*(100-ALPHA_A)+a_observe*ALPHA_A)/100;
			}else{
				if( p_alpha_target[alpha_cnt].update_cnt <= FILTER_VALID_CNT ){
					p_alpha_target[alpha_cnt].update_cnt = 0;
				}
			}
			
			p_alpha_target[alpha_cnt].rIdx      = r_now;
			p_alpha_target[alpha_cnt].vIdx      = v_now;
			p_alpha_target[alpha_cnt].sinPhiIdx = a_now;
			
			if( p_alpha_target[alpha_cnt].rIdx >= (p_epc->rangeMax<<12)*EX_FACTOR ){
				p_alpha_target[alpha_cnt].rIdx = ((p_epc->rangeMax-1)<<12)*EX_FACTOR;
			}
			if( p_alpha_target[alpha_cnt].vIdx >= ((p_epc->chirpMax /2)<<12)*EX_FACTOR ){
				p_alpha_target[alpha_cnt].vIdx = ((p_epc->chirpMax/2-1)<<12)*EX_FACTOR;
			}else if( p_alpha_target[alpha_cnt].vIdx < ((-p_epc->chirpMax/2)<<12)*EX_FACTOR ){
				p_alpha_target[alpha_cnt].vIdx = ((-p_epc->chirpMax/2)<<12)*EX_FACTOR;
			}

			if( p_alpha_target[alpha_cnt].sinPhiIdx >= ((p_epc->angleMax/2)<<12)*EX_FACTOR_A ){
				p_alpha_target[alpha_cnt].sinPhiIdx = ((p_epc->angleMax/2-1)<<12)*EX_FACTOR_A;
			}else if( p_alpha_target[alpha_cnt].sinPhiIdx < ((-p_epc->angleMax/2)<<12)*EX_FACTOR_A ){
				p_alpha_target[alpha_cnt].sinPhiIdx = ((-p_epc->angleMax/2)<<12)*EX_FACTOR_A;
			}
		}
	}

	// 删除重复点
	for( int i=0;i<p_alpha_filter->cnt;i++ ){
			if( p_alpha_target[i].tick_start != 0 ){ // 尚未删除
					for( int j=i+1;j<p_alpha_filter->cnt;j++ ){
							if( p_alpha_target[j].tick_start != 0 ){ // 尚未删除
								if( memcmp((uint8_t*)&p_alpha_target[i],(uint8_t*)&p_alpha_target[j],sizeof(epc_unit_t)-2) == 0 ){ // 依赖epc_unit结构体定义
										//udsf_uart_send_string("delete\n");
										if( p_alpha_target[i].tick_start > p_alpha_target[j].tick_start ){
													p_alpha_target[i].tick_start = 0;
												  
													if( p_alpha_target[i].sn_rcd > p_alpha_target[j].sn_rcd ){
                              p_alpha_target[j].sn_rcd = p_alpha_target[i].sn_rcd;
                          }
													if( p_alpha_target[i].update_cnt > p_alpha_target[j].update_cnt ){
															p_alpha_target[j].update_cnt = p_alpha_target[i].update_cnt;
													}
													break;
											}else{
													p_alpha_target[j].tick_start = 0;
													
													if( p_alpha_target[j].sn_rcd > p_alpha_target[i].sn_rcd ){
															p_alpha_target[i].sn_rcd = p_alpha_target[j].sn_rcd;
													}
													if( p_alpha_target[j].update_cnt > p_alpha_target[i].update_cnt ){
															p_alpha_target[i].update_cnt = p_alpha_target[j].update_cnt;
													}
											}
									}
							}
					}
			}
	}
	
	// 去断点
	int new_cnt=0;
	for( int alpha_cnt=0;alpha_cnt<p_alpha_filter->cnt;alpha_cnt++){
			if( (p_alpha_target[alpha_cnt].tick_start != 0 ) && (CSTD_LLABS((int64_t)sn - (int64_t)p_alpha_target[alpha_cnt].sn_rcd) < FILTER_TIMEOUT_CNT )){
				//udsf_uart_send_string("bbb\n");
					if( new_cnt != alpha_cnt){
							p_alpha_target[new_cnt] = p_alpha_target[alpha_cnt];
					}
					p_alpha_target[new_cnt].unprocess_flag = 0;
					new_cnt++;
			}
	}
	p_alpha_filter->cnt = new_cnt;
	
	// 添加新点
	for( int in=0;in<p_epc->target_cnt;in++){
			if( p_epc_target[in].unprocess_flag == 0 || p_epc_target[in].presence_flag ){ // 只用未处理动点
					continue;
			}
			// 未匹配原始点 增加为独立新点。
			if( p_alpha_filter->cnt < p_alpha_filter->filter_num_max ){
				
					int filter_cnt = p_alpha_filter->cnt;
					for( int alpha_cnt=0;alpha_cnt<p_alpha_filter->cnt;alpha_cnt++){
						if( p_alpha_target[alpha_cnt].unprocess_flag != 0 ){ // 只与新添加点比较
							if(  (CSTD_ABS((int)(p_epc_target[in].rIdx*EX_FACTOR)         - (int)(p_alpha_target[alpha_cnt].rIdx) )       < FILTER_RTH ) &&
									 (CSTD_ABS((int)(p_epc_target[in].vIdx*EX_FACTOR)         - (int)(p_alpha_target[alpha_cnt].vIdx) )       < FILTER_VTH ) &&
									 (CSTD_ABS((int)(p_epc_target[in].sinPhiIdx*EX_FACTOR_A)  - (int)(p_alpha_target[alpha_cnt].sinPhiIdx) )  < FILTER_ATH ) ){
									continue; // 不重复添加相近点
							}
						}
					}

					*(epc_unit_t*)&p_alpha_target[filter_cnt] = *((epc_unit_t*)&p_epc_target[in]);
					p_alpha_target[filter_cnt].rIdx      *= EX_FACTOR;
					p_alpha_target[filter_cnt].vIdx      *= EX_FACTOR;
					p_alpha_target[filter_cnt].sinPhiIdx *= EX_FACTOR_A;

					
//					LOG_SEND_U32_BASE16(LOG_LEVEL_EMERG,1,(uint32_t*)&p_alpha_target[filter_cnt].rIdx,1,"n rId:","\n");
//					LOG_SEND_U32_BASE16(LOG_LEVEL_EMERG,1,(uint32_t*)&p_alpha_target[filter_cnt].vIdx,1,"n vId:","\n");
//					LOG_SEND_U32_BASE16(LOG_LEVEL_EMERG,1,(uint32_t*)&p_alpha_target[filter_cnt].sinPhiIdx,1,"n phiId:","\n");

					
					p_alpha_target[filter_cnt].unprocess_flag = 1; // 新点标记为未处理
					
					p_alpha_target[filter_cnt].info1         = info1;
					//p_alpha_target[filter_cnt].info2         = (p_alpha_filter->color_sn++)%16;
					p_alpha_target[filter_cnt].sn_rcd        = sn+1;        // 记录当前sn
					p_alpha_target[filter_cnt].tick_start    = sn+1;    // 记录当前sn 避免为0
					p_alpha_target[filter_cnt].update_cnt    = 0;
					//p_alpha_target[filter_cnt].presence_flag = 0;
					
					// 新建轨迹 查找未使用color_sn
					uint8_t new_color_sn;
					int find_new_color_ok;
					
					for( int color_i=0;color_i<p_alpha_filter->filter_num_max;color_i++ ){
						new_color_sn = (p_alpha_filter->color_sn%p_alpha_filter->filter_num_max+color_i)%p_alpha_filter->filter_num_max;
						
						find_new_color_ok = 0;
						for( int i=0;i<p_alpha_filter->cnt;i++ ){
								if( p_alpha_target[filter_cnt].info2 == new_color_sn ){
								find_new_color_ok = -1;
								break;
							}
						}
						if( find_new_color_ok == 0 ){
							break;
						}
					}
					
					if( find_new_color_ok == 0 ){
						p_alpha_filter->color_sn = new_color_sn%p_alpha_filter->filter_num_max;
					}
					
					p_alpha_target[filter_cnt].info2         = (p_alpha_filter->color_sn++)%p_alpha_filter->filter_num_max;
					
					p_alpha_filter->cnt++;
			}
	}
	
	// 保留静点
	{
		int new_cnt = 0;
		for( int i=0;i<p_epc->target_cnt;i++ ){
				if( p_epc_target[i].presence_flag ){
						if( new_cnt != i ){
								p_epc_target[new_cnt] = p_epc_target[i];
						}

						p_epc_target[new_cnt].info1       = (info1&(~(2<<6))) | (2<<6);
						p_epc_target[new_cnt].info2       = 0;

						p_epc_target[new_cnt].rIdx       *= EX_FACTOR;
						p_epc_target[new_cnt].vIdx       *= EX_FACTOR;
						p_epc_target[new_cnt].sinPhiIdx  *= EX_FACTOR_A;

						new_cnt ++;
				}
		}
		p_epc->target_cnt = new_cnt;
	}
	
	// 追加动点
	for( int i=0;i<p_alpha_filter->cnt;i++ ){
		if( p_alpha_target[i].update_cnt > FILTER_VALID_CNT ){ // 新点更新N次以上才输出
			//udsf_uart_send_string("fff\n");
			if( p_epc->target_cnt < p_epc->targetMax ){
				//udsf_uart_send_string("ggg\n");
					*((epc_unit_t*)&p_epc_target[p_epc->target_cnt]) = *(epc_unit_t*)&p_alpha_target[i];
					p_epc->target_cnt++;
			}else{
					break;
			}
		}
	}

	return 0;
}

#endif

#endif
